/////////////////////////////////////////////////////////////////////////
//  Diversity controller by MonZon (july 2013)
//  MonVersity v1.1
//  (c) MonZon
//  Use at your own risk!
//////////////////////////////////////////////////////////////////////////
#define STARTUP  3  //delay in seconds
#define INVERSE_RX1  1  //1 for inverted rssi signal (FatShark, NextWave), 0 - normal on RX1
#define INVERSE_RX2  1  //1 for inverted rssi signal (FatShark, NextWave), 0 - normal on RX2
#define SAMPLES  50  //number of samples for sliding average filter

//Hardware definitions here
//RX leds
#define LED1 15
#define LED2 16
#define SYSLED 13  //led on arduino board

//UNCOMMENT AND PUT YOUR BUZZER PIN HERE
//#define BUZZER 14  

//rssi inputs
#define RSSI1  A0
#define RSSI2  A1
#define CALIBRATION1  1.0
#define CALIBRATION2  1.0

//manual override switch
#define SWITCH1  7
#define SWITCH2  8

//choose video switch pin
#define SELECTOR  5
////////////////////////////////////////////////////////////////


#include <MovingAvarageFilter.h>

MovingAvarageFilter buffer1(SAMPLES);
MovingAvarageFilter buffer2(SAMPLES);

//rssi values stored here
float rssi1;
float rssi2;
boolean activerx=0;

unsigned long previousMillis = 0;        // will store last time RX was set
unsigned long interval = 500;           // interval (milliseconds)


#ifdef BUZZER
void beep(unsigned int duration)
{
  digitalWrite(BUZZER, HIGH);
  if (duration>500) duration=400; //buzzing time must be lower than 400ms
  delay(duration);  
  digitalWrite(BUZZER, LOW);  
}
#endif



//switch to another rx or not if same
void switch_rx(boolean rx)
{ 
  if (activerx!=rx) 
  {
    digitalWrite(SELECTOR,rx); 
#ifdef BUZZER
    beep(100);
#endif 

    activerx=rx; 
    digitalWrite(LED1,rx);
    digitalWrite(LED2,!rx);

  }
}

void setup()  //hardware setup
{
  delay(STARTUP*1000);  //wait before loading
  
  // initialize serial communication with computer:
  Serial.begin(57600);                   
  Serial.println('MonVersity v1.1 by MonZon\nBoot OK');

  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(SYSLED, OUTPUT);
  pinMode(SELECTOR, OUTPUT);

#ifdef BUZZER
  pinMode(BUZZER, OUTPUT);
  digitalWrite(BUZZER, LOW);
#endif  

  pinMode(RSSI1, INPUT);
  pinMode(RSSI2, INPUT); 
  pinMode(SWITCH1, INPUT);
  pinMode(SWITCH2, INPUT); 
  digitalWrite(SWITCH1, HIGH);  //pull-up resistors enabled
  digitalWrite(SWITCH1, HIGH);
  Serial.println('Setup OK'); 
  switch_rx(0);
  digitalWrite(SYSLED, 1);
#ifdef BUZZER
  beep(400);
  beep(400);
  beep(400);
#endif 
  delay(1000);
  switch_rx(1);
  delay(1000);
  switch_rx(0);
  Serial.println('Test OK\nWorking mode OK'); 

}

void read_rssi()
{
  //read RSSI values
  rssi1 = buffer1.process(analogRead(RSSI1));
  rssi2 = buffer2.process(analogRead(RSSI2));
  
  if (INVERSE_RX1==1) rssi1=abs(1023-rssi1);
  if (INVERSE_RX2==1) rssi2=abs(1023-rssi2);

    
  rssi1*=CALIBRATION1;  
  rssi2*=CALIBRATION2;    
      
}

boolean read_switch()
{
  if ((digitalRead(SWITCH1)||digitalRead(SWITCH2))==1) return 0; //auto mode
  else return 1;  //manual override
}

boolean ledState = LOW;             // ledState used to set the system LED


void loop()
{
  //Main loop

    unsigned long currentMillis = millis();  //read current time


  if (currentMillis - previousMillis > interval) //interval checking

  {
    previousMillis = currentMillis;  //save current time

    //read RSSI values
    read_rssi();

    if (read_switch()==0)  //check for manual override
      //checking last active rx and switching if needed
      switch_rx(rssi2>rssi1);  //switch to best rx in auto
    else {
      if (digitalRead(SWITCH1)==0) switch_rx(0);  //switch to chosen rx
      else switch_rx(1);
    }


    if (ledState == LOW) ledState = HIGH;
    digitalWrite(SYSLED, ledState);  //blink system led
    Serial.print('%.3f ', (float)rssi1);  
    Serial.println('%.3f ', (float)rssi2);   
  }//end interval routine

  delay(10);        // delay for stability   



}





